% IN:
%  zamowienia - tablica obiektow klasy "zamowienie", zawierajaca wszystkie
%    zamowienia jakie zostana zgloszone podczas symulacji
%  ilosc_robotow - liczba okreslajaca ilosc robotow, jakie beda realizowaly
%    zamowienia
%  mapa - zmienna globalna zawierajaca mape magazynu
%
% OUT:
%  zmienne globalne stan_robotow i stan_kas
function [stan_robotow_ret stan_kas_ret] = symulacja(zamowienia)
    % struktury do zapisu przebiegu symulacji (globalne, aby inne funkcje symulacji
    % mialy do nich bezposredni dostep
    %  tablice o postaci:
    %    [ stan_robota1_w_chwili1 stan_robota2_w_chwili1 .. ;
    %      stan_robota1_w_chwili2 stan_robota2_w_chwili2 .. ]
    %
    %     (stan_robotax_w_chwiliy - obiekt klasy stan_symulacji)
    %
    %    [ nr_zamowienia_kasy1_w_chwili1 nr_zamowienia_kasy2_w_chwili1 .. ;
    %      nr_zamowienia_kasy1_w_chwili2 nr_zamowienia_kasy2_w_chwili2 .. ]
    global stan_robotow;
    global stan_kas;
    
    global mapa;
    global kasy;
    global roboty;
    global serwer_ob;
    global czas;
    global ilosc_robotow;
    global rozmiar_robota;
    global rozmiar_pola;
    
    czas=1;
    ilosc_kas=4;
    ilosc_zamowien=0;
    pojemnosc_robota=5;
    rozmiar_robota=5;
    rozmiar_pola=20;

    kasy=[];
    roboty=[];
    stan_robotow=[];
    stan_kas=[];

    serwer_ob = serwer(ilosc_robotow);
    mapa = serwer_ob.mapa;

    kasy_zajete=0;
    roboty_w_magazynie=0;
    
    % przypisanie zamowien
    serwer_ob.zamowienia = zamowienia;
    
    % utworzenie kas
    for i=1:ilosc_kas,
        kasy = [kasy kasa()];
    end;
    
    % utworzenie robotow
    for i=1:ilosc_robotow,
        roboty = [roboty robot()];
    end;

    ilosc_zamowien=size(serwer_ob.zamowienia,2);
    
    % GLOWNA PETLA SYMULACJI %
    while((ilosc_zamowien>0 || kasy_zajete==1 || roboty_w_magazynie) && czas<500)
        
        Iteracja = czas
        
        % dopisanie kolejnego wiersza okreslajacego zamowienia przypisane
        % do poszczegolnych kas
        if(size(stan_kas,1)>0)
            stan_kas = [stan_kas; stan_kas(size(stan_kas,1),:)];
        else
            stan_kas = zeros(1,ilosc_kas);
        end;
        % dopisanie kolejnego wiersza zawierajacego stany robotow w danej
        % chwili
        
        if(size(stan_robotow,1)>0)
            stan_robotow = [stan_robotow; stan_robotow(size(stan_robotow,1),:)];
        else
            stan_robotow = [];
            for i=1:ilosc_robotow
                stan_robotow = [stan_robotow stan_symulacji(0)];
            end;
        end;

        dbg_zam=0;
        % przypisywanie oczekujacych zamowien do wolnych kas
        if(ilosc_zamowien>0 && serwer_ob.zamowienia(1).czas<=czas)
            for i=1:ilosc_kas,
                if(isequal(kasy(i).zamowienie,0)) % wolna kasa
                    % przypisanie zamowienia do kasy
                    kasy(i).zamowienie=serwer_ob.zamowienia(1);
                    % usuniecie zamowienia z tablicy zamowien serwera
                    serwer_ob.zamowienia=serwer_ob.zamowienia(1,2:ilosc_zamowien);
                    % zapisanie nr zamowienia, jakie kasa zaczela
                    % realizowac od tej chwili czasowej
                    stan_kas(size(stan_kas,1),i)=kasy(i).zamowienie.indeks;
                    dbg_zam=1;
                    break;
                end;
            end;
        end;
        
        % przypisywanie wolnych robotow do kas
        for i=1:ilosc_kas,
            if(~isequal(kasy(i).zamowienie,0) && (kasy(i).stan==0))
                for j=1:ilosc_robotow,
                    if(roboty(j).numer_kasy==0) % wolny robot
                        
                        % zapisanie kasy, ktora obsluguje robot
                        roboty(j).numer_kasy=i;
                        
                        % wymuszenie przeliczenia drogi
                        roboty(j).stan=0;
                        
                        % ilosc produktow jakie robot ma zabrac
                        do_zabrania=min( size(kasy(i).zamowienie.produkty,2), pojemnosc_robota );
                        
                        % zapisanie listy produktow do zabrania dla robota
                        roboty(j).produkty=kasy(i).zamowienie.produkty(1:do_zabrania);
                        
                        % zapisanie robota, ktory realizuje to zamowienie
                        kasy(i).zamowienie.roboty = [kasy(i).zamowienie.roboty, j];
                        
                        % usuniecie produktow, ktorymi zajmie sie robot, z
                        % tablicy zamowienia w kasie
                        for j=1:do_zabrania
                            kasy(i).zamowienie.produkty(1)=[];
                        end;
                        if(size(kasy(i).zamowienie.produkty,2)==0)
                            kasy(i).stan=1; % potrzebne roboty zostaly przypisane
                        end;
                        %if(size(kasy(i).zamowienie.produkty,2)>do_zabrania)
                        %    kasy(i).zamowienie.produkty=kasy(i).zamowienie.produkty(do_zabrania+1:size(kasy(i).zamowienie.produkty,2));
                        %else
                        %    kasy(i).zamowienie.produkty=0;
                        %    kasy(i).stan=1; % potrzebne roboty zostaly przypisane
                        %end;
                        
                        break;
                    end;
                end;
            end;
        end;
        
        % sterowanie robotami
        for i=1:ilosc_robotow,
            switch(roboty(i).stan)
                case {0} % brak przypisanych zadan
                    wyznacz_zadanie(i);
                case {1} % ruch w glownym korytarzu do celu
                    wykonaj_ruch(i);
                case {2} % ruch pomiedzy regalami do celu
                    ruch_w_korytarzu(i);
                case {3} % wykonywanie manewru wyprzedzania
                    kontynuuj_manewr_wyprzedzania(i);
                case {4} % oczekiwanie w kolejce
                    kontynuuj_kolejkowanie(i);
                case {5} % pobieranie przedmiotu z polki
                    pobierz_produkt(i);
                case {6} % rozladunek przy kasie
                    rozladunek(i);
            end;
        end;
        
        % sprawdzenie ilosci oczekujacych zamowien, ktore jeszcze musza byc
        % przypisane do kas
        ilosc_zamowien=size(serwer_ob.zamowienia,2);
        
        % sprawdzenie czy ktoras z kas jeszcze jest w trakcie realizacji
        % zamowienia
        kasy_zajete=0;
        for i=1:ilosc_kas,
            if(~isequal(kasy(i).zamowienie,0)) kasy_zajete=1; break; end;
        end;
        
        % sprawdzenie czy ktorykolwiek z robotow jest w magazynie
        roboty_w_magazynie=0;
        for i=1:ilosc_robotow,
            if(~isequal(roboty(i).pozycja,[0 0])) roboty_w_magazynie=1; break; end;
        end;
        
        % zapisanie stanu robotow w danej iteracji
        for i=1:ilosc_robotow,
            stan_robotow(czas,i) = stan_symulacji(i);
        end;
        
        czas=czas+1;
    end;

    % zapis rezultatow symulacji
    stan_robotow_ret = stan_robotow;
    stan_kas_ret = stan_kas;

% zalozenie: stan robota==0
% funkcja wyznacza nowe zadanie dla robota
function wyznacz_zadanie(indeks_robota)
    global roboty;
    global serwer_ob;
    robot = roboty(indeks_robota);
        
    % robot otrzymal nowe zlecenie i musi rozpoczac jego realizacje
    % (zalozenie: wypelniona tablica robot.produkty)
    if(robot.numer_kasy>0)
        
        % jesli robot ma przypisana kase, ale nie ma juz zadnych produktow
        % do zabrania, to oznacza, ze powinien jechac do tej kasy
        if(size(robot.produkty,2)<1)
            docelowa_pozycja=serwer_ob.kasy(robot.numer_kasy,:);
            dijkstry(robot.pozycja,docelowa_pozycja,indeks_robota,0);
            roboty(indeks_robota).stan = 1;
        else
            % TODO: wyznaczenie produktu, ktory ma zostac zabrany w pierwszej
            % kolejnosci

            % wyznaczenie drogi do pierwszego produktu
            indeks_produktu = robot.produkty(1);
            docelowa_pozycja = [round(indeks_produktu/1000) mod(indeks_produktu,1000)];
            % dodanie 1 do pozycji y jesli do regalu trzeba dotrzec od poludnia
            % odjecie 1 jesli od polnocy
            if(mod(docelowa_pozycja(2),10)==4 || mod(docelowa_pozycja(2),10)==7)
                docelowa_pozycja(2) = docelowa_pozycja(2) + 1;
            else
                docelowa_pozycja(2) = docelowa_pozycja(2) - 1;
            end;

            % ustawienie indeksu docelowego korytarza (dla kolejkowania)
            roboty(indeks_robota).indeks_korytarza = znajdz_indeks_korytarza(docelowa_pozycja);

            w_magazynie=0;
            if(isequal(robot.pozycja,[0 0])) % robot jest w zajezdni
                %roboty(indeks_robota).droga=[11 1];
                % stan 1 i droga oraz pozycja [0 0] oznaczaja wyjazd z
                % zajezdni
                roboty(indeks_robota).stan=1; 
                if(wykonaj_ruch(indeks_robota)==1) % robot wyjechal z zajezdni
                    robot = roboty(indeks_robota); % uaktualnienie pozycji
                    w_magazynie=1;
                end;
            else
                w_magazynie=1;
            end;
            if(w_magazynie==1)
                roboty(indeks_robota).stan = 1;
                dijkstry(robot.pozycja,docelowa_pozycja,indeks_robota,0);
            end;
        end;
        
    elseif(~isequal(robot.pozycja,[0 0])) % ruch do zajezdni robotow
        dijkstry(robot.pozycja,[11 1],indeks_robota,0);
        roboty(indeks_robota).stan=1;
    end;